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Abstract We noticed that if INS data is used as system equations of a Kalman filter 
algorithm for integrated direct geo-referencing, one encounters with a dynamic errors-in- 
variables (DEIV) model. Although DEIV model has been already considered for obser¬ 
vation equations of the Kalman filter algorithm and a solution namely total Kalman filter 
(TKF) has been given to it, this model has not been considered for system equations 
(dynamic model) of the Kalman filter algorithm. Thus, in this contribution, for the first 
time we consider DEIV model for both observation equations and system equations of the 
Kalman filter algorithm and propose a least square prediction namely integrated total 
Kalman filter in contrast to the TKF solution of the previous approach. The variance matrix 
of the unknown parameters are obtained. Moreover, the residuals for all variables are 
predicted. In a numerical example, integrated direct geo-referencing problem is solved for 
a GPS-INS system. 

Keywords Dynamic errors-in-variables • System equations • Integrated 
total Kalman filter • Direct geo-referencing 


1 Introduction 

Recently, there has been an explosion in the number, type and diversity of system designs 
and application areas of mobile sensors. The geo-referencing of these systems is one of the 
main problems. In this problem, one aims to determine the position and attitude of a mobile 
sensor in a geo-referenced frame. When this information is attained directly by means of 
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measurements from sensors on-board the vehicle the term direct geo-referencing is used 
(Skaloud 1999). The integration of these data is done during a Kalman filter algorithm 
(Kalman 1960). For more details on Kalman filter one may refer to Sorenson (1966) and 
Maybeck (1979). The Kalman filter is essentially a set of mathematical equations that 
implement a predictor-corrector type estimator that is optimal in the sense that it mini¬ 
mizes the estimated error covariance, when some presumed conditions are met (Welch and 
Bishop 2001). In the literature, the Kalman filter is derived as either a best predictor (BP) 
or a best linear predictor (BLP), see e.g. Kalman (1960), Gelb (1974), Sanso (1986). The 
minimum mean squared error (MMSE) is the criterion which selects the best predictor or 
estimator. 

Observation equations and system equations are two main parts of a dynamic problem. 
The former is in fact a relation between the observations and time dependent unknown 
parameters while the latter relates the unknown parameters at an epoch i to an earlier epoch 
i — 1. Due to how these two parts are modeled, several linear and non-linear Kalman filters 
have been proposed. For more information see e.g. Yi (2007). Some filters are as follows: 
the Sigma Point Kalman Filters (SPKF) (van der Merwe and Wan 2003) or Linear 
Regression Kalman Filters (LRKF) (Lefebvre et al. 2002), Extended Kalman Filter (EKF) 
(Jazwinski 1970), the Particle Filters (PF) (Liu and Chen 1998), the Ensemble Kalman 
Filter (EnKF) (Evensen 1994), Unscented Kalman Filter (UKF) based on unscented 
transformation (UT) (Julier and Uhlmann 1997) and etc. However, in all of these algo¬ 
rithms, the coefficient matrix of the system equations does not contain random errors. As 
such an assumption cannot always be guaranteed, we allow random observational errors to 
enter the respective matrix. In practice, this situation can be seen when we are going to use 
INS data as the system equations since in such a case, the random observed angular 
increments and velocity increments measured by gyroscope and accelerator of the INS 
system, make the coefficient matrix of the system equations noisy. 

Note that although Schaffrin and Iz (2008), Schaffrin and Uzun (2011) and Mahboub 
et al. (2016) considered the case which only the design matrix of the observation equations 
is random, we solve the problem which both of the coefficient matrix of the observation 
equations and system equations are corrupted by random noise. Hence in contrast to 
Schaffrin and Iz (2008) that named their solution total Kalman filter (TKF), we propose an 
integrated total Kalman filter (ITKF) algorithm. 

This paper is organized as follows: in Sect. 2, the DEIV model and the TKF solution 
proposed by Schaffrin and Iz (2008) are introduced. In Sect. 3, the ITKF algorithm is 
developed, then, in a later section, a numerical example gives insight into the efficiency of 
the algorithm proposed. Finally we conclude the paper. 


2 Dynamic errors-in-variables (DEIV) model 

In this section the concepts of dynamic errors-in-variables (DEIV) model are introduced 
and a TKF solution proposed by Schaffrin and Iz (2008) is given. It must be mentioned that 
EIV model in its time invariant case i.e. static case has been investigated by several 
valuable publications. Therefore, we only give some references e.g. Zeng et al. (2015), 
Zhang et al. (2013), Neitzel (2010), Neitzel and Schaffrin (2016), Snow and Schaffrin 
(2012), Shen et al. (2011), Schaffrin et al. (2014), Schaffrin and Felus (2008), Mahboub 
(2012, 2014, 2016), Mahboub et al. (2012, 2015), Mahboub and Sharifi (2013a, b), Palancz 
and Awange (2012), Amiri-simkooei and Jazaeri (2012), Fang (2011, 2013, a, b c, 2015), 


<£) Springer 



Acta Geod Geophys 


Fang et al. (2015, 2016), Lu et al. (2014), Zhou and Fang (2015) and Fang and Wu (2015) 
etc. In the rest of this paper we define these two parts for a DEIV model. Observation 
equations is given as follows: 

y i ={M-E A )x i + e i (1) 

In the above equations y is the m x 1 random observation vector, e t is the m x 1 vector 
of observational noise, A; is the m x n coefficient matrix of input variables (observed), E A . 
is the corresponding the m x n matrix of random noise, x t is the n x 1 random parameter 
vector (time dependent unknowns). The following equation represents system equations 
which is also called dynamic model. It relates the unknown parameters at an epoch i to an 
earlier epoch i — 1. 

— (®i ~ K$.)xi -1 -\-fi + Uf ( 2 ) 

<&i is the transition matrix E is the corresponding the n x n matrix of random noise and 
u t is the random system noise, is an independent time variable function and underlining 

(_) indicates random variables. The random noise of the transition matrix is our main 

problem in this paper. We also assume that the state vector is observed at an initial 
(previous) epoch: 

*i-i = + e?_! (3) 

Here, is the random noise at the first epoch. Equations (l)-(3) represent the 
functional model of the DEIV model in this paper. We also define the corresponding 
stochastic model as follows: 
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where Q yi , 0 t , Ym-i > Q Ai and Q <*>. are the corresponding dispersion matrixes of the 
observation vector, system equations, the observed unknown parameters at an initial epoch, 
the random coefficient matrix E A . and the random coefficient matrix E 0j . Schaffrin and Iz 
(2008) supposed that E 0 . = Oft = 0, Qa, = 4 0 Q yi and set the following target function: 

®(e i ,e A .,ki,v i )-={^ i Qy'e i +i Ai (I n ®Q y )j e A . 

+ («, - <%?_i) 1r (Or + <?, EL i ^0 " 1 & ') (5} 

+22.J (y ,- A,-(u t — | +i ( ) + —+i ( ) £,•) 

where 2/ is a m x 1 vector of Lagrange multipliers. They obtained the following least- 
squares prediction and named it total Kalman filter (TKF): 

*i — *i + (Oi + ^ \y h + *i QyYi^j j (6) 

where x t and 2/ are given as follows: 
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K={Qy) \yi -AiXi)(l + xjx) 1 (7) 

Xi = ( 8 ) 

As the assumption E 0 . = 0 may not be always correct in particular when the system 
Eqs. (2) are produced by INS data, in the next section we obtain a new solution to this 
problem. 


3 Integrated total Kalman filter (ITKF) 


In this section we solve the DEIV model given by Eqs. (l)-(4). Since we suppose that both 
of the coefficient matrixes in the observation equations and system equations are noisy i.e. 
E 0 . / 0 and E A . / 0, we call our least-squares prediction “integrated total Kalman filter 
(ITKE)”. If we want to use condition equations for our optimization, we require combining 
Eqs. (l)-(3). For this aim, first we insert Eq. (3) into Eq. (1) as follows: 

Xi = (<Pi - Eq) ( x ; _! - £_) +/' + ui (9) 


Then we put Eq. (9) into Eq. (1): 

y. = (A, - Ej ((*, - Eq) - el) +fi + u) + e_, (10) 

Eventually we can set the following least-squares target function: 

«<-i) := ejQ~ 1 e i + ^1^.+ uJer l Ui+ el.Qo,) + e?_i(X^-i) 

+ 2/1 f (y t - e t - (Ai - £ A .) ((#,• - Eq) (x^ - el) + u)) 

( 11 ) 


Note that in contrast to target function of Eq. (5) proposed by Schaffrin and Iz (2008), 
the target function given by Eq. (11) can produce the predicted residuals of all random 
observed variables. In Schaffrin and Iz (2008) the quantities u\ an e^_ x were not predicted. 
For optimization, if tildas ( ~ ) indicate predicted vectors and hats ( ^ ) denote estimated 
ones the following necessary conditions must hold: 

ei,e Ai ,l h e^iii,e f^g = 2^Q~ l ei -=0 (12) 
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(17) 


and £ 4 * can be obtained from Eqs. (12) and (13) as follows 

ei = QyXi ( 18 ) 

e Ai = -Qa, ,(((<?! - £<p,) (*;_i - e?_i) +/• + Mi) ® 4)X- = -2 a,^ 4/ (19) 

Equations (14) and (15) immediately lead to 

e», = -2i>, (fe-i - «?_i) ® (Ai - A/)!,- = -g*M- (20) 

«/ = 0/(Ai-£ Al ) 7 % (21) 


Equation (16) gives ej_ } as follows: 

el -, = ,( (A; - A,) (*, - £*,) ) ^ (22) 

Eventually by inserting Eqs. (18)-(22) into Eq. (17), the vector of Lagrange multipliers 
Xi can be estimated as follows: 

yi - QyM - (A; - E Aj )9i(Ai - E A ) T li - S'l'Q^SjA, 

- (A, - E Ai )zl, ((A, - E Ai ) (0, - E^fli - At&tx^ 

- ((#.■*,■-1 +fd® I m)QA i Rih-A i fj = 0 -> 
h = {Q yt + (A; - £a,)0«(A ; - E A ) T +S]Q 9i Si (23) 

+ (A, - ((A; - A) (*i - £*,)) r +((^--l +/) r ® / m)2A,2?,)” 1 

(yi - M&iXj-i +fi )) 

In the above equation, the inverse exists since the matrix 5,- is full column rank i.e. its 
quadratic form is invertible. After prediction of random observed variables ei,eA n e$.,Ui 
and ^ ) _ 1 iteratively using Eqs. (18)-(23), we must update the measured unknown param¬ 
eters x t _ x and the corresponding dispersion matrix for the next epoch i. By applying 
variance propagation rules to Eq. (9), the updated dispersion matrix for the next epoch is 
given by 
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D(Xi) = K i .blkdiag(D(x i _ 1 ),D(el l ),D(u i ),D{E $l )).Kj 


(24) 


With 


K, = 




dxj dx ( dx; dxj 


X) 


-l l^-i ~ e i-\) 


dx t _ i 0e9_j dm dE<p. 

'n)} 


= [(0 i -E„ i ) -(0,-Et,) In 


From Eq. (9) the update of the unknown parameters x t is obtained as follows: 

Xi = (tp >i - Eq) (x t _ x - e?_j) +/■ + Ui 


(25) 


Thus the update part for the next epoch is given by Eqs. (24) and (25). Summarizing, we 
propose the ITKF algorithm by the following flowchart: 
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4 ITKF algorithm for integrated direct geo-referencing 


If we want to produce the system equations by INS data for integrated direct geo-refer¬ 
encing, one has to consider Eq. (2) as the system equations where the coefficient matrix &i 
is noisy i.e. E 0 . / 0. In order to sense this condition, we must examine the mathematical 
model of an INS system. It is obtained after solving navigation equations. For a back¬ 
ground one may refer to Sheta (2012) or Jekeli (2001). Navigation equations are a set of 
differential equations which describe the input gyroscopes and accelerometers measure¬ 
ments input to the local frame mechanization and the output curvilinear coordinates, three 
velocity components, and three attitude components. Input gyroscopes are angular incre¬ 
ments which are measured by IMU. Solving these vector differential equations, through 
integration, will result in a time variable state vector with kinematic sub-vectors for 
position, velocity, and attitude. The input to computation process are the angular incre¬ 
ments measured by gyroscope and the velocity increments measured by accelerometer. The 
rotation matrix is updated by following Eq. (26). The Quaternion approach is used in the 
update because it deals with the singularity problems of the Euler angles at the 90 degrees 
angle. The quaternion is a 4 elements vector represented in space and contains the 
amplitude in one element and the direction is described using the three remaining elements. 
In general, the system equations can be described by the following equation 
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etween two epochs and Pf — [ cp t hi ] is position 
\ denotes quaternion rotations. The noisy coefficients a, b, c 
and d are provided by the observed angular increments and the updated velocity V 4 is 
produced by the observed velocity increments. 

Consequently, the noisy coefficient matrix <P h the unknown parameters x t and the vector 
fi introduced in Eq. (2) are as follows: 
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Now suppose that for an integrated geo-referencing of a mobile sensor, we are going to 
determine the position and attitude of a mobile sensor at five epochs. Due to Eqs. (27)- 
(29), the components of the DEIV model of the system equations at these epochs are as 
follows: 
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For all of the DEIV models of these system equations, the stochastic model is given by 


— {h 0 q)(l 7 0 q ) T ; q = 10 
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Note that for i — 0,1,2,.. .6 the (7/ + 1) to (li + 3 )th. rows and columns of the matrix 
{Ii 0 q) must be replaced by zero. 

The observation equations which can be produced by GPS and remote sensed data are 
given by 5 DEIV models at 5 epochs i = 1, 2, 3, 4, 5 as 
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11.511 

0.41599 

0.043181 

-10.4 

7.0747 

0.61147 

-11.546 

0.27357 

0.15443 

0.045528 

1.6455 

-0.6405 

0.033169 - 

-0.023809 

2.2963 

6.5331 

-0.31565 

5.7747 

-0.059541 

-0.44762 

2.462 

-0.14936 

0.082947 

0.14955 

20.582 

0.32071 

-0.12439 

-2.6433 

2.4281 

5.2551 

0.43379 


For all of the DEIV models of the observation equations, the stochastic model is given by 
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Qx = (h ®q)(h ®q) T ',q 
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0 
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0 
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0.01 

0.1 

0.02 
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0.1 

0.06 

0.1 

0.1 

0.2 

0.03 

0.4 

0.06 

0.1 

1 

0.6 

0.1 

0 

0.4 

0.07 

0.1 

0.04 

0.2 

0.4 

0.6 

0.1 

0.1 

0.1 

0.03 

0.5 

0.06 

0.6 

0.4 

0.3 

2 

1 

0.6 

0.01 

1 

0.06 

1 

1 

0.6 

0.1 

1 


For i — 0,1,2,.. .6 the (9 i + 1) to (9 i + 3 )th rows and columns of the matrix (4 0 q) must 
be replaced by zero. 


- 69.06 

9.78 

33.69 

16.73 

9.15 

5.58 

29.6 

5.53 

9.11 - 

9.78 

80.44 

-9.24 

5.57 

8.97 

5.08 

24.29 

2.14 

0.48 

33.69 

-9.24 

57.78 

2.28 

23.12 

5.43 

33.16 

18.54 

12.08 

16.73 

5.57 

2.28 

49.13 

1.45 

1.58 

1.03 

1.42 

0.97 

9.15 

8.97 

23.12 

1.45 

37.34 

1.172 

2.07 

4.68 

5.32 

5.58 

5.08 

5.43 

1.58 

1.172 

2.31 

7.34 

2.82 

1.76 

29.6 

24.29 

33.162 

1.03 

2.07 

7.34 

117.3 

43.52 

6.88 

5.53 

2.14 

18.54 

1.42 

4.68 

2.82 

43.52 

26.11 

4.64 

_ 9.11 

0.48 

12.08 

0.97 

5.32 

1.76 

6.88 

4.64 

3.91 _ 


Also the observed state vector x t at an initial epoch with its corresponding dispersion 
matrix is given by: 


"4.01 

0.4 

0.1 

0 

0.0 

0.0 

0.0 - 


" 103.01 " 

0.4 

5 

3 

0 

0.0 

0.0 

0.0 


132.9 

0.1 

3 

2 

0 

0.0 

0.0 

0.0 


166 

0.0 

0.0 

0.0 

0.01 

0.0 

0.0 

0.0 

,Xi 

-0.57 

0.0 

0.0 

0.0 

0 

0.01 

0.0 

0.0 


-0.16 

0.0 

0.0 

0.0 

0 

0.0 

0.01 

0.0 


-0/57 

_ 0.0 

0.0 

0.0 

0 

0.0 

0.0 

0.01 _ 


0.56 


In this problem both of the observation equations and system equations are in fact DEIV 
models. Three algorithms KF, TKF and ITKF are applied to this problem. We compare the 
result with true solution which are illustrated by Figs. 1 and 2 for 3-D position and attitude 
of the mobile sensor in a local frame respectively. The results demonstrated that the 
proposed ITKF approach can significantly improve the solution of the predicted position 
and attitude in contrast to other algorithms. Note that after computing the attitudes in 
quaternion representation, we converted them into three rotations about three axis in 
degrees. The improvement of the predicted position is more considerable than the pre¬ 
dicted attitude. However, the TKF solution has larger difference with respect to true 
solution than the ITKF solution since it does not consider the random property of the 
random design matrix This situation gets worse for the KF solution in which not only 
we neglect the random property of the noisy design matrix but also the random design 
matrix A t is considered deterministic i.e. with no noise. Moreover, the general treatment of 
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Fig. 1 solutions of different algorithms for 3-D position of the mobile sensor in a local frame 



Fig. 2 solutions of different algorithms for 3-D attitude of the mobile sensor in a local frame 


the TKF and ITKF approach are similar, however, we can see a significant bias in the TKF 
solution respect to the ITKF solution which is because of inappropriate modeling of the 
system equations made by the TKF approach, particularly when the magnitude of the 
weights of the elements in the random design matrixes A t and cannot be neglected. 
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5 Conclusions and outlook 

In this paper, we developed a new Kalman filter algorithm. Its main assumption is that the 
system equations of a dynamic problem can itself be a DEIV model i.e. the design matrix 
of the system equations is also noisy. In practice one can see this situation when the 
system equations are provided by INS data. In such a case, the random noises are produced 
by observed angular increments and velocity increments. The predicted residuals for all 
variables besides the variance matrix of the unknown parameters were obtained by the 
proposed ITKF algorithm. In a numerical example, it was shown that the proposed ITKF 
approach can make the best improvement in solution in contrast to other algorithms, if both 
of the coefficient matrixes in the observation equations and the system equations are noisy. 
The prediction part is done by Eqs. (18)—(23) and the update part for the next epoch is 
given by Eqs. (24) and (25). In the forthcoming publication, we try to improve the pre¬ 
diction part due to several practical vulnerabilities of direct geo-referencing problem. 
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